#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "wtk_nth_biggest_element.h"

#define _ISORT_MAX 5

#define _COMP(a,b) (((a)>(b))?1:0)

//[first, last) sort
static void insertion_sort(float * first, float * last)
{
    float * next, *next1,* first1;
    float value;
    if(first != last)
    {
        for(next = first; ++next != last;)
        {
            value = *next;
            if(_COMP(value, *first))
            {
                memmove(first + 1, first, ((unsigned char *)next) - ((unsigned char *)first));
                *first = value;
            }else{
                next1 = next;
                for(first1 = next1;_COMP(value, *--first1); next1 = first1)
                {
                    *next1 = * first1;
                }
                *next1 = value;
            }
        }
    }
}

static inline uint32_t  _uint32_log2(uint32_t n)
{
#define S(k) if(n>= (1u<<k)){i+=k;n>>=k;}
    int i = -(n==0);S(16);S(8);S(4);S(2);S(1);return i;
#undef S
}

static inline void _iter_swap(float * a, float * b)
{
    float c;
    c = *a;
    *a = *b;
    *b = c;
}

static inline void _move_median3_to_first(float * result, float * a, float * b, float * c)
{
    if(_COMP(*a, * b))
    {   
        if(_COMP(*b, *c))
            _iter_swap(result, b);
        else if(_COMP(*a, *c))
            _iter_swap(result, c);
        else
            _iter_swap(result, a);
    }else if(_COMP(*a, *c)){
        _iter_swap(result, a);
    }else if(_COMP(*b, *c)){
        _iter_swap(result, c);
    }else{
        _iter_swap(result, b);
    }
}


static inline float * _unguarded_partition(float * first, float * last, float * pivot)
{
    while(1)
    {
        while(_COMP(*first, *pivot))
            ++first;
        --last;
        while(_COMP(*pivot, *last))
            --last;

        if(!(first < last))
            return first;

        _iter_swap(first, last);
        ++first;
    }
}

static inline int _move_median5_to_first_stride(float * first, float * last, int stride)
{
    int i;
    static float *aux[5];
    float * tmp;
    float * it;
    
    for(i=0, it = first;it < last&&i<5; it += stride, ++i)
    {
        aux[i] = it;
        switch(i)
        {
            case 4:
                if(*(aux[4]) < *(aux[3]))
                {
                    tmp = aux[3];
                    aux[3] = aux[4];
                    aux[4] = tmp;
                }else{
                    break;
                }
            case 3:
                if(*(aux[3]) < *(aux[2]))
                {
                    tmp = aux[2];
                    aux[2] = aux[3];
                    aux[3] = tmp;
                }else{
                    break;
                }
            case 2:
                if(*(aux[2]) < *(aux[1]))
                {
                    tmp = aux[1];
                    aux[1] = aux[2];
                    aux[2] = tmp;
                }else{
                    break;
                }
            case 1:
                if(*(aux[1]) < *(aux[0]))
                {
                    tmp = aux[0];
                    aux[0] = aux[1];
                    aux[1] = tmp;
                }
            default:
                break;
        }
    }
    if(i == 5)
    {//swap median to first
        _iter_swap(aux[2],first);
        return 1;
    }else if(i>1){
            _iter_swap(aux[(i-1)/2],first);
        return -1;
    }else 
    {
        return -1;
    }
}

static inline void _median_of_medians(float * first, float *last)
{
    int count = 5;
    int stride = 1;
    int next_stride;
    float * next;
    while(count > 1)
    {
        count = 0;
        next = first;
        next_stride = stride * 5;
        while(next < last)
        {
            _move_median5_to_first_stride(next, last, stride);
            next += next_stride;
            count += 1;
        }
        stride = next_stride;
    }
}

static void bfprt_select(float * first, float * nth, float * last)
{
    float *cut;
    while(_ISORT_MAX < last - first)
    {
        _median_of_medians(first, last);

        cut = _unguarded_partition(first + 1, last, first);
        if(cut <= nth)
            first = cut;
        else    
            last = cut;
    }
    insertion_sort(first, last);
}

static void print_array(float * first, float * last)
{
    while(first < last)
    {   
        fprintf(stderr,"%f\t", *first++);
    }   
    fprintf(stderr,"\n");
}


static void introselect(float * first, float * nth, float * last, int depth_limit)
{
    float * cut;
    float * mid;
    while(_ISORT_MAX < last -first)
    {
#if 1
        if(depth_limit == 0)
        {
            //use true median
            bfprt_select(first, nth,last);
            return;
        }
#endif

        --depth_limit;

        //med3
        mid = first + (last - first)/2;
        _move_median3_to_first(first, first + 1, mid, last -1);
        cut = _unguarded_partition(first + 1 , last, first);

        if(cut <= nth)
            first = cut;
        else    
            last = cut;
    }
    insertion_sort(first, last);
}

void wtk_nth_biggest_element(float * first, float * nth, float * last)
{
    if(first == last || nth==last)
    {
        return;
    }
    //print_array(first, last);
    introselect(first, nth, last, (_uint32_log2(last - first)<<1));
}

